home *** CD-ROM | disk | FTP | other *** search
/ AmigActive 23 / AACD 23.iso / AACD / Sound / delfinampeg / src / delfinampeg.c < prev    next >
C/C++ Source or Header  |  2001-06-14  |  24KB  |  696 lines
  1. /*****************************************************************************
  2.  
  3.     delfinampeg.device - mpeg.device for Delfina DSP
  4.     Copyright (C) 2000, 2001  Michael Henke
  5.  
  6.     This program is free software; you can redistribute it and/or modify
  7.     it under the terms of the GNU General Public License as published by
  8.     the Free Software Foundation; either version 2 of the License, or
  9.     (at your option) any later version.
  10.  
  11.     This program is distributed in the hope that it will be useful,
  12.     but WITHOUT ANY WARRANTY; without even the implied warranty of
  13.     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14.     GNU General Public License for more details.
  15.  
  16.     You should have received a copy of the GNU General Public License
  17.     along with this program; if not, write to the Free Software
  18.     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19.  
  20. *****************************************************************************/
  21.  
  22.  
  23. /*#define DEBUG*/
  24.  
  25. #include <proto/exec.h>
  26. #include <proto/dos.h>
  27. #include <exec/memory.h>
  28. #include <exec/devices.h>
  29. #include <exec/errors.h>
  30. #include <exec/interrupts.h>
  31. #include <devices/melodympeg.h>
  32. #include <dos/var.h>
  33.  
  34. #ifdef DEBUG
  35. extern void KPutStr(STRPTR);
  36. extern LONG KPutChar(LONG);
  37. #endif
  38.  
  39. extern struct DelfObj DSP56K_PCM;
  40. extern struct DelfObj DSP56K_MP2;
  41. extern struct DelfObj DSP56K_MP3;
  42. #include <libraries/delfina.h>
  43. #include "PCM.h"
  44. #include "MP2.h"
  45. #include "MP3.h"
  46.  
  47. /** static LONG pow43tab[8206]; **/
  48. #include "MP3_pow43tab.h"
  49. /** static LONG quantab[512]; **/
  50. #include "MP3_quantab.h"
  51.  
  52. extern char DevName;    /* "delfinampeg.device" */
  53. extern struct Library *DelfinaBase; /* initialized by device Init routine */
  54.  
  55. struct devunit
  56. {
  57.     struct Unit unit;
  58.     ULONG   unitnum;
  59.     struct List ioreqlist;
  60.     struct IOMPEGReq *currioreq;
  61.     UBYTE   *currpt;
  62.     ULONG   currlen;
  63.     ULONG   layer, freqidx, mono, firstheader;
  64.     ULONG   volumeleft, volumeright, pause, initdelf_ok;
  65.     BYTE    cleanup_flag, cleanup_count;
  66.     struct DelfPrg *prg_pcm, *prg_mp2, *prg_mp3;
  67.     struct DelfModule *mod_pcm;
  68.     DELFPTR mem_il_mp2, mem_ip_mp2;
  69.     DELFPTR mem_il_mp3, mem_ip_mp3, mem_x_mp3_ro, mem_y_mp3_quantab;
  70.     struct Interrupt delfint;
  71.     ULONG   intkey;
  72.     UBYTE   framebuf[4096], bitresbuf[4096];
  73.     UWORD   bitresoffset, bitresok, framebufstate;
  74.     UWORD   framebufoffset, framebufleft, III_main_data_size;
  75.     UWORD   delfcopysize, II_translate, II_jsbound, modext;
  76.     UWORD   II_forcemono, III_forcemono, forcemono;
  77.     ULONG   II_dacrate, III_dacrate, dacrate;
  78.     UBYTE   *delfcopypt;
  79. };
  80.  
  81. /* possible values for 'framebufstate' */
  82. #define FBS_GETHEADER           0
  83. #define FBS_GETFRAMEDATA        1
  84. #define FBS_FILLED              2
  85.  
  86. #define MPG_MD_STEREO           0
  87. #define MPG_MD_JOINT_STEREO     1
  88. #define MPG_MD_DUAL_CHANNEL     2
  89. #define MPG_MD_MONO             3
  90. #define HDR_MPEG1               0xfff80000
  91. #define HDR_CONSTANT            0xfffe0c00  /* layer, sampling frequency */
  92.  
  93. void* C_initunit(ULONG unitnum);
  94. ULONG C_expungeunit(struct devunit *unit);
  95. void  C_setpause(struct devunit *unit, struct IOMPEGReq *iomr);
  96. void  C_setvolume(struct devunit *unit, struct IOMPEGReq *iomr);
  97. void  C_reset(struct devunit *unit);
  98. void  C_flush(struct devunit *unit);
  99. ULONG C_write(struct devunit *unit, struct IOMPEGReq *iomr);
  100. static ULONG initDelfina(struct devunit *unit);
  101. static void  cleanupDelfina(struct devunit *unit);
  102. static void __asm IntServer(register __a1 struct devunit *unit);
  103. static UWORD GetBits(UWORD num);
  104.  
  105. static struct TagItem tagdone={TAG_DONE,0};
  106. static ULONG volumeleft=0x400000, volumeright=0x400000;  /* default: 100% */
  107. static UWORD *gb_pt=NULL, gb_buf=0, gb_num=0;
  108.  
  109. static const ULONG mpgfreq[4]={44100,48000,32000,0};
  110. static const UWORD mpgbitrate[3][16]=
  111.         { {0,32,64,96,128,160,192,224,256,288,320,352,384,416,448,0}, /* I */
  112.           {0,32,48,56,64,80,96,112,128,160,192,224,256,320,384,0},   /* II */
  113.           {0,32,40,48,56,64,80,96,112,128,160,192,224,256,320,0} }; /* III */
  114. static const UBYTE mp2translate[3][2][16] =
  115.         { { { 0,2,2,2,2,2,2,0,0,0,1,1,1,1,1,0 } ,    /* 44100 stereo */
  116.             { 0,2,2,0,0,0,1,1,1,1,1,1,1,1,1,0 } } ,  /* 44100 mono   */
  117.           { { 0,2,2,2,2,2,2,0,0,0,0,0,0,0,0,0 } ,    /* 48000 stereo */
  118.             { 0,2,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } } ,  /* 48000 mono   */
  119.           { { 0,3,3,3,3,3,3,0,0,0,1,1,1,1,1,0 } ,    /* 32000 stereo */
  120.             { 0,3,3,0,0,0,1,1,1,1,1,1,1,1,1,0 } } }; /* 32000 mono   */
  121. static const UBYTE mp2sblimit[4]={27,30,8,12};
  122.  
  123.  
  124.  
  125. /** called by OpenDevice() **/
  126. void* C_initunit(ULONG unitnum)
  127. {
  128.     struct devunit *u;
  129. #ifdef DEBUG
  130. KPutStr("C_initunit\n");
  131. #endif
  132.     u=(struct devunit*)AllocVec(sizeof(struct devunit),MEMF_PUBLIC|MEMF_CLEAR);
  133.     if(!u) return(NULL);
  134.     u->unit.unit_MsgPort.mp_Node.ln_Type=NT_MSGPORT;
  135.     u->unit.unit_MsgPort.mp_Node.ln_Name= &DevName;
  136.     u->unit.unit_MsgPort.mp_Flags=PA_IGNORE;
  137.     NewList(&u->unit.unit_MsgPort.mp_MsgList);
  138.     u->unitnum=unitnum;
  139.     u->volumeleft=volumeleft;
  140.     u->volumeright=volumeright;
  141.     NewList(&u->ioreqlist);
  142.     {
  143.         UBYTE *varbuf;
  144.         LONG  varlen, args[4]={0,0,0,0};
  145.         struct RDArgs *rdargs, *rdargs2;
  146.         struct DosLibrary *DOSBase;
  147.         if((DOSBase=(struct DosLibrary*)OpenLibrary("dos.library",37)))
  148.         {
  149.             if((varbuf=AllocVec(1024,MEMF_PUBLIC|MEMF_CLEAR)))
  150.             {
  151.                 if((varlen=GetVar("DELFINAMPEG",varbuf,1024,LV_VAR))>0)
  152.                 {
  153. #ifdef DEBUG
  154. KPutStr("DELFINAMPEG var: '"); KPutStr(varbuf); KPutStr("'\n");
  155. #endif
  156.                     if((rdargs=AllocDosObject(DOS_RDARGS,&tagdone)))
  157.                     {
  158.                         rdargs->RDA_Source.CS_Buffer=varbuf;
  159.                         rdargs->RDA_Source.CS_Length=varlen;
  160.                         rdargs->RDA_Source.CS_CurChr=0;
  161.                         rdargs->RDA_Flags=RDAF_NOPROMPT;
  162.                         rdargs->RDA_DAList=0;
  163.                         rdargs2=ReadArgs("L2MONO/S,L2RATE/K/N,L3MONO/S,L3RATE/K/N",args,rdargs);
  164.                         if(args[0]) u->II_forcemono=1;
  165.                         if(args[1]) u->II_dacrate=(*(LONG*)args[1]);
  166.                         if(args[2]) u->III_forcemono=1;
  167.                         if(args[3]) u->III_dacrate=(*(LONG*)args[3]);
  168.                         FreeArgs(rdargs2);
  169.                         FreeDosObject(DOS_RDARGS,rdargs);
  170.                     }
  171.                 }
  172.                 FreeVec(varbuf);
  173.             }
  174.             CloseLibrary((struct Library*)DOSBase);
  175.         }
  176.     }
  177.     return(u);
  178. }
  179.  
  180.  
  181.  
  182. /** called by CloseDevice() **/
  183. ULONG C_expungeunit(struct devunit *u)
  184. {
  185.     ULONG unitnum=u->unitnum;
  186. #ifdef DEBUG
  187. KPutStr("C_expungeunit\n");
  188. #endif
  189.     C_flush(u);
  190.     FreeVec(u);
  191.     return(unitnum);
  192. }
  193.  
  194.  
  195.  
  196. /** called by MPEGCMD_PAUSE **/
  197. void C_setpause(struct devunit *u, struct IOMPEGReq *iomr)
  198. {
  199. #ifdef DEBUG
  200. KPutStr("C_setpause\n");
  201. #endif
  202.     u->pause=iomr->iomr_Arg1;
  203. }
  204.  
  205.  
  206.  
  207. /** called by MPEGCMD_SETAUDIOPARAMS **/
  208. void C_setvolume(struct devunit *u, struct IOMPEGReq *iomr)
  209. {
  210.     struct MPEGAudioParams *ap;
  211. #ifdef DEBUG
  212. KPutStr("C_setvolume\n");
  213. #endif
  214.     ap=(struct MPEGAudioParams*)iomr->iomr_Req.io_Data;
  215.     if(ap)
  216.     {
  217.         u->volumeleft =volumeleft =(0x400000*((ap->map_VolumeLeft >>8)&0xff))/0xff;
  218.         u->volumeright=volumeright=(0x400000*((ap->map_VolumeRight>>8)&0xff))/0xff;
  219.         if(u->prg_pcm)
  220.         {
  221.             Delf_Run(u->prg_pcm->prog+PROG_PCM_INIT, 0, 0,
  222.                  (ULONG)(u->forcemono ? 1 : u->mono),
  223.                  u->volumeleft,
  224.                  mpgfreq[u->freqidx],
  225.                  u->volumeright );
  226.         }
  227.     }
  228. }
  229.  
  230.  
  231.  
  232. /** called by CMD_RESET **/
  233. void C_reset(struct devunit *u)
  234. {
  235. #ifdef DEBUG
  236. KPutStr("C_reset--");
  237. #endif
  238.     u->volumeleft=u->volumeright=volumeleft=volumeright=0x400000;
  239.     u->pause=0;
  240.     C_flush(u);
  241. }
  242.  
  243.  
  244.  
  245. /** called by CMD_FLUSH **/
  246. void C_flush(struct devunit *u)
  247. {
  248.     struct IOMPEGReq *iomr;
  249. #ifdef DEBUG
  250. KPutStr("C_flush\n");
  251. #endif
  252.     cleanupDelfina(u);
  253.     u->currlen=0; u->currpt=NULL; 
  254.     u->layer=u->freqidx=u->mono=u->firstheader=0;
  255.     u->bitresoffset=u->bitresok=0;
  256.     u->framebufstate=FBS_GETHEADER;
  257.     if((iomr=u->currioreq))
  258.     {
  259.         iomr->iomr_Req.io_Error=IOERR_ABORTED;
  260.         ReplyMsg(&iomr->iomr_Req.io_Message);
  261.         u->currioreq=NULL;
  262.     }
  263.     while((iomr=(struct IOMPEGReq*)RemHead(&u->ioreqlist)))
  264.     {
  265.         iomr->iomr_Req.io_Error=IOERR_ABORTED;
  266.         ReplyMsg(&iomr->iomr_Req.io_Message);
  267.     }
  268. }
  269.  
  270.  
  271.  
  272. /** called by CMD_WRITE **/
  273. ULONG C_write(struct devunit *u, struct IOMPEGReq *iomr)
  274. {
  275. #ifdef DEBUG
  276. /*KPutStr("C_write\n");*/
  277. #endif
  278.     if( (iomr->iomr_Req.io_Length==0) ||
  279.         (iomr->iomr_Req.io_Data==NULL) ||
  280.         (iomr->iomr_StreamType!=MPEGSTREAM_AUDIO) )
  281.     {
  282.         iomr->iomr_Req.io_Error=MPEGERR_BAD_PARAMETER;
  283.         return(1); /*ERROR*/
  284.     }
  285.     Disable();
  286.     AddTail(&u->ioreqlist, (struct Node*)iomr);
  287.     Enable();
  288. #ifdef DEBUG
  289. KPutStr("C_write...AddTail\n");
  290. #endif
  291.     /*even if initDelfina() returns an error we must not return it  */
  292.     /*here because the request is already queued in our ioreqlist.  */
  293.     /*(the requests in ioreqlist will be replied by C_flush() later)*/
  294.     if(!u->initdelf_ok) initDelfina(u);
  295.     return(0);
  296. }
  297.  
  298.  
  299.  
  300. /** called by C_write() **/
  301. static ULONG initDelfina(struct devunit *u)
  302. {
  303.     while(!u->firstheader)
  304.     {
  305.         ULONG header, i;
  306.         UBYTE *pt, *ptmax;
  307. #ifdef DEBUG
  308. KPutStr("initDelfina...firstheader\n");
  309. #endif
  310.         /** find frame header and extract some info: layer, freqidx, mono **/
  311.         if(!u->currioreq)
  312.         {
  313.             if(!(u->currioreq=(struct IOMPEGReq*)RemHead(&u->ioreqlist))) return(1); /*ERROR*/
  314.             u->currpt=u->currioreq->iomr_Req.io_Data;
  315.             u->currlen=u->currioreq->iomr_Req.io_Length;
  316.         }
  317.         pt=u->currpt; ptmax=pt+u->currlen; header=0;
  318. retry_firstheader:
  319.         i=0;
  320.         while(pt<ptmax)
  321.         {
  322.             header=(header<<8)|(ULONG)(*pt++);
  323.             if( ((header&HDR_MPEG1)==HDR_MPEG1) &&  /*sync*/
  324.                 (((header>>17)&3)!=0) &&            /*layer!=4*/
  325.                 (((header>>17)&3)!=3) &&            /*layer!=1*/
  326.                 (((header>>12)&15)!=0) &&           /*bitrate!=0*/
  327.                 (((header>>12)&15)!=15) &&          /*bitrate!=15*/
  328.                 (((header>>10)&3)!=3)               /*freqidx!=3*/
  329.               ) {i=1; break;} /*pattern match*/
  330.         }
  331.         if(i) /*found something!*/
  332.         {
  333.             /**check next header (for safer recognition!)**/
  334.             i=((ULONG)mpgbitrate[3-((header>>17)&3)][(header>>12)&15]*144000)/mpgfreq[(header>>10)&3]+((header>>9)&1)-4;
  335.             if((pt+i)>=(ptmax-4)) goto retry_firstheader; /*beyond this buffer!*/
  336.             i=((ULONG)(*(pt+i))<<24)|((ULONG)(*(pt+i+1))<<16)|((ULONG)(*(pt+i+2))<<8)|(ULONG)(*(pt+i+3));
  337.             if( ((i&HDR_CONSTANT)!=(header&HDR_CONSTANT)) ||
  338.                 (((i>>12)&15)==0) ||
  339.                 (((i>>12)&15)==15) ) goto retry_firstheader; /*header mismatch!*/
  340.             /**now we are quite sure that it really is an MPEG frame header**/
  341.             u->currpt=pt-4;
  342.             u->currlen=ptmax-pt+4;
  343.             u->firstheader=header;
  344.             u->layer=4-((header>>17)&3);
  345.             u->freqidx=(header>>10)&3;
  346.             u->mono=( (((header>>6)&3)==MPG_MD_MONO) ? 1 : 0 );
  347.  
  348.             u->forcemono= u->layer==2 ? u->II_forcemono : u->III_forcemono;
  349.             u->dacrate= u->layer==2 ? u->II_dacrate : u->III_dacrate;
  350.         }
  351.         else /*not found*/
  352.         {
  353. #ifdef DEBUG
  354. KPutStr("initDelfina...firstheader...MPEGERR_CMD_FAILED\n");
  355. #endif
  356.             u->currioreq->iomr_Req.io_Error=MPEGERR_CMD_FAILED;
  357.             u->currioreq->iomr_MPEGError=MPEGEXTERR_STREAM_MISMATCH;
  358.             ReplyMsg(&u->currioreq->iomr_Req.io_Message);
  359.             u->currioreq=NULL; u->currpt=NULL; u->currlen=0;
  360.         }
  361.     }
  362.  
  363.  
  364.     if(!u->prg_pcm)
  365.     {
  366. #ifdef DEBUG
  367. KPutStr("initDelfina...prg_pcm\n");
  368. #endif
  369.         if(!(u->prg_pcm=Delf_AddPrg(&DSP56K_PCM))) return(1); /*ERROR*/
  370.         Delf_Run(u->prg_pcm->prog+PROG_PCM_INIT, 0, 0,
  371.                  (ULONG)(u->forcemono ? 1 : u->mono),
  372.                  u->volumeleft,
  373.                  mpgfreq[u->freqidx],
  374.                  u->volumeright );
  375.     }
  376.  
  377.     if(u->layer==2)
  378.     {
  379.         if(!u->prg_mp2)
  380.         {
  381. #ifdef DEBUG
  382. KPutStr("initDelfina...prg_mp2\n");
  383. #endif
  384.             if(!u->mem_il_mp2) u->mem_il_mp2=Delf_AllocMem(INTL_MP2_DATL, DMEMF_LDATA|DMEMF_INTERNAL|DMEMF_ALIGN_64);
  385.             if(!u->mem_ip_mp2) u->mem_ip_mp2=Delf_AllocMem(INTP_MP2_PROG, DMEMF_PROG|DMEMF_INTERNAL);
  386.             if(!(u->prg_mp2=Delf_AddPrg(&DSP56K_MP2))) return(1); /*ERROR*/
  387.             Delf_Run(u->prg_mp2->prog+PROG_MP2_INIT, 0, 0, u->mem_il_mp2, u->mem_ip_mp2, 0, 0);
  388.             Delf_Poke(u->prg_mp2->ydata+DATY_MP2_FORCEMONO, DMEMF_YDATA, (ULONG)u->forcemono);
  389.         }
  390.     }
  391.     else if(u->layer==3)
  392.     {
  393.         if(!u->mem_x_mp3_ro)
  394.         {
  395.             if(!(u->mem_x_mp3_ro=Delf_AllocMem(EXTX_MP3_RO,DMEMF_XDATA))) return(1); /*ERROR*/
  396.         }
  397.         if(!u->mem_y_mp3_quantab)
  398.         {
  399.             if(!(u->mem_y_mp3_quantab=Delf_AllocMem(EXTY_MP3_QUANTAB,DMEMF_YDATA))) return(1); /*ERROR*/
  400.             Delf_CopyMem(quantab, (void*)u->mem_y_mp3_quantab, 512*4, DCPF_FROM_AMY|DCPF_YDATA|DCPF_32BIT);
  401.         }
  402.         if(!u->prg_mp3)
  403.         {
  404. #ifdef DEBUG
  405. KPutStr("initDelfina...prg_mp3\n");
  406. #endif
  407.             if(!u->mem_il_mp3) u->mem_il_mp3=Delf_AllocMem(INTL_MP3_DATL, DMEMF_LDATA|DMEMF_INTERNAL|DMEMF_ALIGN_64);
  408.             if(!u->mem_ip_mp3) u->mem_ip_mp3=Delf_AllocMem(INTP_MP3_PROG, DMEMF_PROG|DMEMF_INTERNAL);
  409.             if(!(u->prg_mp3=Delf_AddPrg(&DSP56K_MP3))) return(1); /*ERROR*/
  410.             Delf_Run(u->prg_mp3->prog+PROG_MP3_INIT, 0, 0, u->mem_il_mp3, u->mem_ip_mp3, u->freqidx, u->mem_x_mp3_ro);
  411.             Delf_CopyMem(pow43tab, (void*)(u->prg_mp3->ydata+DATY_MP3_POW43TAB), 8206*4, DCPF_FROM_AMY|DCPF_YDATA|DCPF_32BIT);
  412.             Delf_Poke(u->prg_mp3->xdata+DATX_MP3_QUANTAB_P, DMEMF_XDATA, u->mem_y_mp3_quantab);
  413.             Delf_Poke(u->prg_mp3->ydata+DATY_MP3_FORCEMONO, DMEMF_YDATA, (ULONG)u->forcemono);
  414.         }
  415.     }
  416.     else return(1); /*ERROR: unsupported layer*/
  417.  
  418.     if(!u->intkey)
  419.     {
  420. #ifdef DEBUG
  421. KPutStr("initDelfina...AddIntServer\n");
  422. #endif
  423.         u->delfint.is_Code=(void(*)(void))IntServer;
  424.         u->delfint.is_Data=u;
  425.         if(!(u->intkey=Delf_AddIntServer(u->prg_pcm->prog+PROG_PCM_INTKEY,&u->delfint))) return(1); /*ERROR*/
  426.     }
  427.     if(!u->mod_pcm)
  428.     {
  429. #ifdef DEBUG
  430. KPutStr("initDelfina...AddModule\n");
  431. #endif
  432.         if(!(u->mod_pcm=Delf_AddModule(DM_Inputs, 0, DM_Outputs, 1,
  433.                             DM_Code, u->prg_pcm->prog+PROG_PCM_MODULE,
  434.                             DM_Freq, u->dacrate ? u->dacrate*1000 : mpgfreq[u->freqidx],
  435.                             DM_Name, &DevName,
  436.                             0 ))) return(1); /*ERROR*/
  437.     }
  438.  
  439.     u->initdelf_ok=1;
  440. #ifdef DEBUG
  441. KPutStr("initDelfina...ok\n");
  442. #endif
  443.     return(0);
  444. }
  445.  
  446.  
  447.  
  448. /** called by C_flush() **/
  449. static void cleanupDelfina(struct devunit *u)
  450. {
  451.     ULONG pause_tmp;
  452. #ifdef DEBUG
  453. KPutStr("cleanupDelfina\n");
  454. #endif
  455.     pause_tmp=u->pause;
  456.     if(u->mod_pcm)
  457.     {
  458.         u->cleanup_flag=1;
  459.         u->cleanup_count=2;
  460.         /** wait until the currently running DSP routine has finished **/
  461.         /** this avoids the nasty deadlock with delfina.library 4.14  **/
  462.         while(u->cleanup_count>0) u->pause=1;
  463.         Delf_RemModule(u->mod_pcm); u->mod_pcm=NULL;
  464.     }
  465.     if(u->intkey)  {Delf_RemIntServer(u->intkey); u->intkey=0;}
  466.     if(u->prg_mp2) {Delf_RemPrg(u->prg_mp2); u->prg_mp2=NULL;}
  467.     if(u->prg_mp3) {Delf_RemPrg(u->prg_mp3); u->prg_mp3=NULL;}
  468.     if(u->prg_pcm) {Delf_RemPrg(u->prg_pcm); u->prg_pcm=NULL;}
  469.     if(u->mem_il_mp2) {Delf_FreeMem(u->mem_il_mp2,DMEMF_LDATA|DMEMF_INTERNAL); u->mem_il_mp2=NULL;}
  470.     if(u->mem_ip_mp2) {Delf_FreeMem(u->mem_ip_mp2,DMEMF_PROG |DMEMF_INTERNAL); u->mem_ip_mp2=NULL;}
  471.     if(u->mem_il_mp3) {Delf_FreeMem(u->mem_il_mp3,DMEMF_LDATA|DMEMF_INTERNAL); u->mem_il_mp3=NULL;}
  472.     if(u->mem_ip_mp3) {Delf_FreeMem(u->mem_ip_mp3,DMEMF_PROG |DMEMF_INTERNAL); u->mem_ip_mp3=NULL;}
  473.     if(u->mem_x_mp3_ro) {Delf_FreeMem(u->mem_x_mp3_ro,DMEMF_XDATA); u->mem_x_mp3_ro=NULL;}
  474.     if(u->mem_y_mp3_quantab) {Delf_FreeMem(u->mem_y_mp3_quantab,DMEMF_YDATA); u->mem_y_mp3_quantab=NULL;}
  475.     u->cleanup_flag=0;
  476.     u->pause=pause_tmp;
  477.     u->initdelf_ok=0;
  478. }
  479.  
  480.  
  481.  
  482. static void __asm IntServer(register __a1 struct devunit *u)
  483. {
  484.     if((u->pause) || (u->framebufstate!=FBS_FILLED))
  485.     {
  486.         if(u->cleanup_flag) u->cleanup_count--;
  487.         else Delf_Run(u->prg_pcm->prog+PROG_PCM_MUTE,0,DRUNF_ASYNCH,0,0,0,0);
  488.     }
  489.     else
  490.     {
  491.         if(u->layer==2)
  492.         {
  493.             if(!Delf_Peek(u->prg_mp2->ydata+DATY_MP2_BUSY,DMEMF_YDATA))
  494.             {
  495.                 Delf_CopyMem( u->delfcopypt,
  496.                               (void*)(u->prg_mp2->xdata+DATX_MP2_INBUF),
  497.                               (ULONG)u->delfcopysize,
  498.                               DCPF_FROM_AMY|DCPF_XDATA|DCPF_24BIT );
  499.                 Delf_Run( u->prg_mp2->prog+PROG_MP2_DECODE, 0, DRUNF_ASYNCH,
  500.                           u->mono,
  501.                           Delf_Peek(u->prg_pcm->ydata+DATY_PCM_BUFPTR,DMEMF_YDATA),
  502.                           (ULONG)u->II_translate,
  503.                           (ULONG)u->II_jsbound );
  504.                 u->framebufstate=FBS_GETHEADER;
  505.             }
  506.         }
  507.         else if(u->layer==3)
  508.         {
  509.             if(!Delf_Peek(u->prg_mp3->ydata+DATY_MP3_BUSY,DMEMF_YDATA))
  510.             {
  511.                 if(u->bitresok)
  512.                 {
  513.                     Delf_CopyMem( &u->bitresbuf[0],
  514.                                   (void*)(u->prg_mp3->xdata+DATX_MP3_INBUF),
  515.                                   (ULONG)(u->III_main_data_size+33),
  516.                                   DCPF_FROM_AMY|DCPF_XDATA|DCPF_24BIT );
  517.                     Delf_Run( u->prg_mp3->prog+PROG_MP3_DECODE, 0, DRUNF_ASYNCH,
  518.                               u->mono,
  519.                               Delf_Peek(u->prg_pcm->ydata+DATY_PCM_BUFPTR,DMEMF_YDATA),
  520.                               (ULONG)u->modext,
  521.                               0 );
  522.                 }
  523.                 u->framebufstate=FBS_GETHEADER;
  524.             }
  525.         }
  526.     }
  527.  
  528.     if(u->framebufstate==FBS_GETHEADER)
  529.     {
  530.         /*find next valid frame header*/
  531.         ULONG fh=u->firstheader&HDR_CONSTANT, ch=0;
  532.         while( ((ch&HDR_CONSTANT)!=fh) ||   /*sync, layer, freqidx*/
  533.                (((ch>>12)&15)==0) ||        /*bitrate!=0*/
  534.                (((ch>>12)&15)==15) )        /*bitrate!=15*/
  535.         {
  536.             if(u->currioreq)
  537.             {
  538.                 if(u->currlen)
  539.                 {
  540.                     ch=(ch<<8)|(ULONG)(*u->currpt);
  541.                     u->currpt++; u->currlen--;
  542.                 }
  543.                 else
  544.                 {
  545.                     u->currioreq->iomr_Req.io_Actual=u->currioreq->iomr_Req.io_Length;
  546.                     ReplyMsg(&u->currioreq->iomr_Req.io_Message);
  547.                     u->currioreq=NULL; u->currpt=NULL; /*u->currlen=0;*/
  548.                 }
  549.             }
  550.             else
  551.             {
  552.                 if(!(u->currioreq=(struct IOMPEGReq*)RemHead(&u->ioreqlist)))
  553.                 {
  554.                     ch=0; break; /*ERROR*/
  555.                 }
  556.                 u->currpt=u->currioreq->iomr_Req.io_Data;
  557.                 u->currlen=u->currioreq->iomr_Req.io_Length;
  558.             }
  559.         }
  560.         if(ch) /*found it!*/
  561.         {
  562.             u->framebufoffset=0;
  563.             u->framebufleft=((ULONG)mpgbitrate[u->layer-1][(ch>>12)&15]*144000)/mpgfreq[(ch>>10)&3]+((ch>>9)&1)-4;
  564.             u->II_translate=mp2translate[u->freqidx][u->mono][(ch>>12)&15];
  565.             if(((ch>>6)&3)==MPG_MD_JOINT_STEREO)
  566.             {
  567.                 u->modext=(ch>>4)&3;
  568.                 u->II_jsbound=(u->modext<<2)+4;
  569.             }
  570.             else
  571.             {
  572.                 u->modext=0;
  573.                 u->II_jsbound=mp2sblimit[u->II_translate];
  574.             }
  575.             u->delfcopysize=u->framebufleft;
  576.             u->delfcopypt= &u->framebuf[0];
  577.             if(!((ch>>16)&1))
  578.             {
  579.                 u->delfcopysize-=2;
  580.                 u->delfcopypt+=2;
  581.             }
  582.             u->framebufstate=FBS_GETFRAMEDATA;
  583.         }
  584.     }
  585.  
  586.     if(u->framebufstate==FBS_GETFRAMEDATA)
  587.     {
  588.         /*fetch frame data*/
  589. #ifdef DEBUG
  590. KPutChar('i');
  591. #endif
  592.         while(u->framebufleft)
  593.         {
  594.             if(u->currioreq)
  595.             {
  596.                 if(u->currlen)
  597.                 {
  598.                     ULONG i=u->currlen;
  599.                     if(u->framebufleft<i) i=u->framebufleft;
  600.                     CopyMem(u->currpt, &u->framebuf[u->framebufoffset], i);
  601.                     u->currpt+=i; u->currlen-=i;
  602.                     u->framebufoffset+=i; u->framebufleft-=i;
  603.                     if(u->framebufleft==0) u->framebufstate=FBS_FILLED;
  604.                 }
  605.                 else
  606.                 {
  607.                     u->currioreq->iomr_Req.io_Actual=u->currioreq->iomr_Req.io_Length;
  608.                     ReplyMsg(&u->currioreq->iomr_Req.io_Message);
  609.                     u->currioreq=NULL; u->currpt=NULL; /*u->currlen=0;*/
  610.                 }
  611.             }
  612.             else
  613.             {
  614.                 if(!(u->currioreq=(struct IOMPEGReq*)RemHead(&u->ioreqlist)))
  615.                 {
  616.                     break; /*ERROR*/
  617.                 }
  618.                 u->currpt=u->currioreq->iomr_Req.io_Data;
  619.                 u->currlen=u->currioreq->iomr_Req.io_Length;
  620.             }
  621.         }
  622.         if((u->framebufstate==FBS_FILLED) && (u->layer==3))
  623.         {
  624.             UWORD md;
  625.             /**extract main data size**/
  626.             gb_pt=(UWORD*)(u->delfcopypt+2); gb_num=0;
  627.             if(u->mono)
  628.             {
  629.                 GetBits(9+5+4-16);
  630.                 md=GetBits(12);
  631.             }
  632.             else
  633.             {
  634.                 GetBits(9+3+8-16);
  635.                 md=GetBits(12);
  636.                 GetBits(59-12);
  637.                 md+=GetBits(12);
  638.                 GetBits(59-12);
  639.                 md+=GetBits(12);
  640.             }
  641.             GetBits(59-12);
  642.             md+=GetBits(12);
  643.             u->III_main_data_size=(md+7)>>3;
  644.             /**bit reservoir handling**/
  645.             {
  646.                 UWORD main_data_begin, i;
  647.                 UBYTE *p0, *p1;
  648.                 p1=u->delfcopypt;
  649.                 main_data_begin= (UWORD)(*p1)<<1 | (UWORD)(*(p1+1))>>7;
  650.                 /* copy side information */
  651.                 p0= &u->bitresbuf[0];
  652.                 for(i=32; i>0; i--) *p0++= *p1++;
  653.                 /* copy previous main_data */
  654.                 p0++; /* &u->bitresbuf[33] */
  655.                 if(u->bitresoffset>=main_data_begin)
  656.                 {
  657.                     u->bitresok=1;
  658.                     p1=p0+u->bitresoffset-main_data_begin;
  659.                     for(i=main_data_begin; i>0; i--) *p0++= *p1++;
  660.                     u->bitresoffset=main_data_begin;
  661.                 }
  662.                 else
  663.                 {
  664.                     u->bitresok=0; /* not enough data in reservoir */
  665.                     p0+=u->bitresoffset;
  666.                 }
  667.                 /* copy current main_data */
  668.                 i=(u->mono) ? 17 : 32; /* side info length */
  669.                 p1=u->delfcopypt+i;    /* begin of main_data area */
  670.                 i=u->delfcopysize-i;   /* mainslots */
  671.                 u->bitresoffset+=i;
  672.                 for(; i>0; i--) *p0++= *p1++;
  673.             }
  674.         }
  675.     }
  676. }
  677.  
  678.  
  679.  
  680. static UWORD GetBits(UWORD num)
  681. {
  682.     UWORD val=0;
  683.     for(; num>0; num--)
  684.     {
  685.         if(gb_num==0)
  686.         {
  687.             gb_buf= *gb_pt++;
  688.             gb_num= 16;
  689.         }
  690.         gb_num--;
  691.         val+= val;
  692.         val|= (gb_buf>>gb_num)&1;
  693.     }
  694.     return(val);
  695. }
  696.